/*
 * Copyright (c) 2006-2021, RT-Thread Development Team
 *
 * SPDX-License-Identifier: Apache-2.0
 *
 * Change Logs:
 * Date           Author       Notes
 * 2021-10-29     MBW       the first version
 */
#include "sys_height.h"
#include "PPM.h"
#include "data_define.h"
#include "maths.h"
#include "delay_uil.h"
height_control height_cmd={0};

void height_start_ppm(void *p)
{
    u8 ord_sta = 1;

    u32 t_now,t_preiod,t_pre ;
    void *delay;
    delay = delay_until_create("delay");  // 创建相对延时

    while(1)
    {
        //实时解析PPM信号值，1为起飞，0为降落
        if(PPM_con.PPM_thrust > 500)
        {
            delay_until_run(delay, 10);
            if(PPM_con.PPM_thrust > 500){height_cmd.state_now=1;
            }
        }
        else {
            delay_until_run(delay, 10);
            if(PPM_con.PPM_thrust < 500){
                height_cmd.state_now=0;
            }
        }
        //**********任务运行时间统计**********
        t_now = rt_tick_get();//要赋值给整型数据
        t_preiod=t_now-t_pre;
        task_time1.ppm_time = 1000/t_preiod;
        t_pre = t_now;
        delay_until_run(delay, 10);
    }
}
void height_start(void *p)
{
    u32 t_now,t_preiod,t_pre ;
    void *delay;
    delay = delay_until_create("delay");  // 创建相对延时
    float A=0,W=0,T_TAR=0,T=0,b=0,cnt=0,A_H=0;
    while(1)
    {
         A = control_data.A/2;
         A_H = control_data.A_H/2;
         T_TAR = control_data.T;
         T = T_TAR*2;
         W = 2*PI/T;
         b = T/4;

         control_data.basethrust = A*sin(W*(cnt-b))+A;
         control_data.height_set = A_H*sin(W*(cnt-b))+A_H;
        if(height_cmd.state_now==1) //上升
        {
            cnt=cnt+0.01;
        }
        else {
            cnt=cnt-0.01;
        }
        cnt = constrainf(cnt,0,T_TAR);

        //**********任务运行时间统计**********
        t_now = rt_tick_get();//要赋值给整型数据
        t_preiod=t_now-t_pre;
        task_time1.height_time = 1000/t_preiod;
        t_pre = t_now;

        delay_until_run(delay, 10);
    }
}
void height_start_init(void)
{


    rt_thread_t tid1 = RT_NULL , tid2 = RT_NULL;
//    control_data.cnt = control_data.T;
    tid1 = rt_thread_create("height_start_ppm",height_start_ppm, RT_NULL, 2000, height_task_prio, 20);
    if (tid1 != RT_NULL)rt_thread_startup(tid1),rt_kprintf("...height_start_ppm is ok!\n");

    tid2 = rt_thread_create("height_start",height_start, RT_NULL, 2000, height_task_prio, 20);
    if (tid2 != RT_NULL)rt_thread_startup(tid2),rt_kprintf("...height_start is ok!\n");
}
INIT_APP_EXPORT(height_start_init);
